#include "Arduino.h"
#include "arduinoPins.h"
#include "board.h"
#include <stdbool.h>

#define HALF_SQUARE 0xdeadbeef //number of steps to rotate a half square

extern Square** board;

void rotate(int steps, float speed, bool mtr);

//If an input is 3+ volts it's HIGH, if it's 2- volts it's LOW
//for an output from arduino, 5 volts is HIGH, 0 volts is LOW
void update_board(Square src, Square dest) {
  int spaces_a = dest.row - src.row;
  int spaces_b = dest.col - src.col;
  int dir_a = (spaces_a > 0) ? 1 : -1;
  int dir_b = (spaces_b > 0) ? 1 : -1;
  float ramp = .1;
  
  int distance_a = abs(spaces_a) * 1900 * 3;
  int distance_b = abs(spaces_b) * 3750 * 3;
  
//  rotate( (src.row * 1900 * 3), .6, true);
//  rotate( (src.col * 3750 * 3), .6, false);
  
//  digitalWrite(MAGNET, HIGH);
  //move a certain number of centimeters
  while(distance_a != 0) {
    rotate((250 * dir_a), ramp, true);
    distance_a = distance_a - 950;
    ramp += 0.3;
  }
  ramp = .1;
  while(distance_b != 0) {
    rotate((250 * dir_b), ramp, false);
    distance_b = distance_b - 1875;
    ramp += 0.3;
  }
//  digitalWrite(MAGNET, LOW);

//  rotate(-(dest.row * 1900 * 3), .6, true);
//  rotate(-(dest.col * 3750 * 3), .6, false);
}

/*
*	helper function, returns {dest row, dest col, src row, src col}
*  dir: 1 is -row, 2 is +row, 3 is -col, 4 is +col
*/
int* nudge(Square square, Square** tmpBoard, int dir){
	int* toReturn = (int*)malloc(4*sizeof(int));
	int row = (int)(square.row);
	int col = (int)(square.col);
	bool nudged = false;
	if(dir==1){//piece moving left
		if(!tmpBoard[row][col+1].occupied){ //nudge up
			update_board(square,tmpBoard[row][col+1]);
                toReturn[0] = row;
                toReturn[1] = col+1;
                toReturn[2] = row;
                toReturn[3] = col;
			tmpBoard[row][col+1].occupied=true;
			nudged=true;
		}
		else if(!tmpBoard[row][col-1].occupied){//nudge down
			update_board(square,tmpBoard[row][col-1]);
                toReturn[0] = row;
                toReturn[1] = col-1;
                toReturn[2] = row;
                toReturn[3] = col;
			tmpBoard[row][col-1].occupied=true;
			nudged=true;
		}
		else if(!tmpBoard[row-1][col].occupied){ //nudge left
			update_board(square,tmpBoard[row-1][col]);
                toReturn[0] = row-1;
                toReturn[1] = col;
                toReturn[2] = row;
                toReturn[3] = col;
			tmpBoard[row-1][col].occupied=true;
			nudged=true;
		}
	}
	else if(dir==2){//piece moving right
		if(!tmpBoard[row][col+1].occupied){ //nudge up
			update_board(square,tmpBoard[row][col+1]);
                toReturn[0] = row;
                toReturn[1] = col+1;
                toReturn[2] = row;
                toReturn[3] = col;
			tmpBoard[row][col+1].occupied=true;
			nudged=true;
		}
		else if(!tmpBoard[row][col-1].occupied){//nudge down
			update_board(square,tmpBoard[row][col-1]);
                toReturn[0] = row;
                toReturn[1] = col-1;
                toReturn[2] = row;
                toReturn[3] = col;
			tmpBoard[row][col-1].occupied=true;
			nudged=true;
		}
		else if(!tmpBoard[row-1][col].occupied){ //nudge right
			update_board(square,tmpBoard[row+1][col]);
                toReturn[0] = row-1;
                toReturn[1] = col;
                toReturn[2] = row;
                toReturn[3] = col;
			tmpBoard[row+1][col].occupied=true;
			nudged=true;
		}
	}
	else if(dir==3){//piece moving down
	if(!tmpBoard[row+1][col].occupied){	//nudge right
		update_board(square,tmpBoard[row+1][col]);
                toReturn[0] = row+1;
                toReturn[1] = col;
                toReturn[2] = row;
                toReturn[3] = col;
		tmpBoard[row+1][col].occupied=true;
		nudged=true;
		}
	else if(!tmpBoard[row-1][col].occupied){ //nudge left
		update_board(square,tmpBoard[row-1][col]);
                toReturn[0] = row-1;
                toReturn[1] = col;
                toReturn[2] = row;
                toReturn[3] = col;
		tmpBoard[row-1][col].occupied=true;
		nudged=true;
		}
	else if(!tmpBoard[row][col+1].occupied){ //nudge down
		update_board(square,tmpBoard[row][col-1]);
                toReturn[0] = row;
                toReturn[1] = col+1;
                toReturn[2] = row;
                toReturn[3] = col;
		tmpBoard[row][col+1].occupied=true;
		nudged=true;
		}
	}
	else if(dir==4){//piece moving up
		if(!tmpBoard[row+1][col].occupied){	//nudge right
			update_board(square,tmpBoard[row+1][col]);
                toReturn[0] = row+1;
                toReturn[1] = col;
                toReturn[2] = row;
                toReturn[3] = col;
			tmpBoard[row+1][col].occupied=true;
			nudged=true;
		}
		else if(!tmpBoard[row-1][col].occupied){ //nudge left
			update_board(square,tmpBoard[row-1][col]);
                toReturn[0] = row-1;
                toReturn[1] = col;
                toReturn[2] = row;
                toReturn[3] = col;
			tmpBoard[row-1][col].occupied=true;
			nudged=true;
		}
		else if(!tmpBoard[row][col+1].occupied){ //nudge up
			update_board(square,tmpBoard[row][col+1]);
                toReturn[0] = row;
                toReturn[1] = col+1;
                toReturn[2] = row;
                toReturn[3] = col;
			tmpBoard[row][col+1].occupied=true;
			nudged=true;
		}
	}
	if(nudged=true)
		tmpBoard[row][col].occupied=false;
	else{//nudge failed
		//TODO:write code for this
	}

	return toReturn;
}

/*
*	helper function that returns number of
*	occupied squares between src and dest
*/
int occuCount(int sRow, int sCol, int dRow, int dCol){
	int toReturn=0;
	int dir,curr;

	if(sRow!=dRow){
		if(sRow>dRow)
			dir=(-1);
		for(curr=dRow; curr!=dCol; curr+=dir){
			if(board[curr][sCol].occupied)
				toReturn++;
		}
		return toReturn;
	}
	if(sCol!=dCol){
		if(sCol>dCol)
			dir=(-1);
		for(curr=sCol; curr!=dCol; curr+=dir){
			if(board[sRow][curr].occupied)
				toReturn++;
		}
		return toReturn;
	}
	return 0;
}

/*
* adds 1d array to 2d array
*/
int** addToList(int** list, int* toAdd, int ind){
	int curr;
	int **toReturn = (int**)malloc(ind*sizeof(int*));

	if(ind=1){
		*toReturn = toAdd;
		return toReturn;
	}
	for(curr=0; curr<ind; curr++)
		toReturn[ind]=list[curr];
	toReturn[ind]=toAdd;
	free(list);
	return toReturn;
}

/*
* An attempt at pathfinding
*/
void remove(Square piece){
	int row,col,dRow,dCol,xDist,yDist,curr,dir,xdir,ydir,tmp,tmp2,nudges;
	int** nudgeList;
	int* toAdd;
	Square** tmpBoard = (Square**)malloc(10*sizeof(Square*));
	Square sSquare,dSquare;
	//init row + col
	row=(int)(piece.row);
	col=(int)(piece.col);

	for(tmp=0; tmp<10; tmp++){
		tmpBoard[tmp]=(Square*)malloc(10*sizeof(Square));
		for(tmp2=0; tmp2<10; tmp2++){
			tmpBoard[tmp][tmp2]=board[tmp][tmp2];
		}
	}

	/* determines coords to move piece to that require
	* the least amount of nudging
	*/
		dRow=row;
		dCol=0;
		tmp=occuCount(row,col,row,0);
		tmp2=occuCount(row,col,row,9);
		if(tmp2<tmp){
			dCol=9;
			dRow=row;
			tmp=tmp2;
		}
		tmp2=occuCount(row,col,0,col);
		if(tmp2<tmp){
			dCol=col;
			dRow=0;
			tmp=tmp2;
		}
		tmp2=occuCount(row,col,9,col);
		if(tmp2<tmp){
			dCol=col;
			dRow=9;
		}



	//src and dest differ only in row coordinates
	if( (row!=dRow) && (col==dCol) ){ //only has to change rows
		nudges=0; //counter for number of nudges
		dir=1;
		xDist=dRow-row; //gets dist has to move

		if((xDist==1)||(xDist==(-1))){//if this passes only has to move one square to get off board
			update_board(piece,board[dRow][dCol]);

			for(tmp = 0; tmp<9; tmp++){
				free(tmpBoard[tmp]);
			}
			free(tmpBoard);

			return;
		}

		if(xDist<0)
			dir=-1;

		for(curr = dir; curr != xDist; curr+=dir){
			toAdd=NULL;
			if(nudge==-1)tmp=1; //tmp=1 means direction is left (from positive to  negative row)
			else tmp=2; //tmp=2 means direction is right (from negative to positive row)
			if(board[row+curr][col].occupied) //determines if square occupied and nudge needs to be performed
				toAdd=nudge(board[row+curr][col],tmpBoard,tmp); //performs nudge and stores the nudge data in toAdd (dest row, dest col, source row, source col)
			if(toAdd!=NULL){
				nudgeList=addToList(nudgeList,toAdd,++nudges); //adds a nudge that was performed to nudge list
			}
		}
		update_board(piece,board[dRow][dCol]); //moves piece to edge now that nudges have been performed/it has clear path

		for(nudges; nudges>0; nudges--){//undoes the nudges that were performed earlier in order like poppin goff stack
			sSquare=board[nudgeList[nudges][0]][nudgeList[nudges][1]]; //gets src of how to undo nudge
			dSquare=board[nudgeList[nudges][2]][nudgeList[nudges][3]]; //gets dest of how to undo nudge
			update_board(sSquare,dSquare); //performs the anti-nudge
			free(nudgeList[nudges]); //frees
		}

		for(tmp = 0; tmp<9; tmp++){
			free(tmpBoard[tmp]);
		}
		free(tmpBoard);
		return;
	}
	if( (col!=dCol) && (row==dRow) ){ //only has to change columns
		dir=1;
		yDist=dCol-row;

		if((yDist==1)||(yDist==(-1))){
			update_board(piece,board[dRow][dCol]);
			for(tmp = 0; tmp<9; tmp++){
				free(tmpBoard[tmp]);
			}
			free(tmpBoard);
			return;
		}

		if(yDist<0)
			dir=-1;

		for(curr = dir; curr != yDist; curr+=dir){
			toAdd=NULL;
			if(dir==-1) tmp=3; //tmp being 3 means down (positive col to neg)
			else tmp=4; //tmp 4 means up (negative col to positive)
			if(board[row][col+curr].occupied)
				toAdd=nudge(board[row+curr][col],tmpBoard,tmp);
			if(toAdd!=NULL){
				nudgeList=addToList(nudgeList,toAdd,++nudges);
			}
		}

		update_board(piece,board[dRow][dCol]);

		for(nudges; nudges>0; nudges--){
			sSquare=board[nudgeList[nudges][0]][nudgeList[nudges][1]];
			dSquare=board[nudgeList[nudges][2]][nudgeList[nudges][3]];
			update_board(sSquare,dSquare);
			free(nudgeList[nudges]);
		}
	}

	for(tmp = 0; tmp<9; tmp++){
		free(tmpBoard[tmp]);
	}
	free(tmpBoard);

	return;
}

void rotate(int steps, float speed, bool mtr) {
  //rotate a specific number of microsteps (8 microsteps per step) - (negative for reverse movement)
  //speed is any number from .01 -> 1 with 1 being fastest - Slower is stronger
  int dir = (steps > 0)? HIGH:LOW;
  int dir_pin = (mtr) ? DIR_PIN_A : DIR_PIN_B;
  int step_pin = (mtr) ? STEP_PIN_A : STEP_PIN_B;
  steps = abs(steps);
  
  digitalWrite(dir_pin, dir);

  if(speed > 1) {
    speed = 1;
  }

  float usDelay = (1/speed) * 70;

  for(int i=0; i < steps; i++){
    digitalWrite(step_pin, HIGH);
    delayMicroseconds(usDelay); 

    digitalWrite(step_pin, LOW);
    delayMicroseconds(usDelay);
  }
}
